//! Odometry Demo Node
//! 
//! This node subscribes to robot pose messages and publishes odometry and TF transformations.
//! It demonstrates how to bridge pose data to ROS2 navigation stack.

use r2r::geometry_msgs::msg::{TransformStamped, PoseStamped};
use r2r::nav_msgs::msg::Odometry;
use r2r::tf2_msgs::msg::TFMessage;
use r2r::QosProfile;
use futures::{executor::LocalPool, stream::StreamExt, task::LocalSpawnExt};
use std::time::{SystemTime, UNIX_EPOCH};

/// Frame IDs used in the odometry system
const ODOM_FRAME: &str = "odom";
const BASE_LINK_FRAME: &str = "base_link";

/// Topic names
const POSE_TOPIC: &str = "/utlidar/robot_pose";
const ODOM_TOPIC: &str = "/odom";
const TF_TOPIC: &str = "/tf";

/// Node name
const NODE_NAME: &str = "odom_demo";

/// Get current system time as seconds since Unix epoch
/// 
/// # Returns
/// Current time as f64 seconds, or 0.0 if system time is invalid
fn get_current_time() -> f64 {
    SystemTime::now()
        .duration_since(UNIX_EPOCH)
        .map(|duration| duration.as_secs_f64())
        .unwrap_or_else(|_| {
            eprintln!("Warning: System time is before Unix epoch, using 0.0");
            0.0
        })
}

/// Convert f64 timestamp to ROS2 Time message
/// 
/// # Arguments
/// * `timestamp` - Time in seconds since Unix epoch
/// 
/// # Returns
/// Tuple of (seconds, nanoseconds) for ROS2 Time
fn timestamp_to_ros_time(timestamp: f64) -> (i32, u32) {
    let secs = timestamp.floor() as i32;
    let nsecs = ((timestamp - secs as f64) * 1e9).round() as u32;
    (secs, nsecs)
}



/// Handle incoming pose messages and publish odometry and TF
/// 
/// # Arguments
/// * `msg` - Incoming PoseStamped message
/// * `odom_puber` - Publisher for odometry messages
/// * `tf_puber` - Publisher for TF messages
fn handle_pose(
    msg: PoseStamped, 
    odom_puber: &r2r::Publisher<Odometry>, 
    tf_puber: &r2r::Publisher<TFMessage>
) -> Result<(), Box<dyn std::error::Error>> {
    let current_time = get_current_time();
    let (secs, nsecs) = timestamp_to_ros_time(current_time);
    
    // Create and publish messages directly to avoid cloning
    publish_tf_and_odometry(tf_puber, odom_puber, msg, secs, nsecs)
}

/// Publish both TF and odometry messages efficiently without unnecessary cloning
fn publish_tf_and_odometry(
    tf_puber: &r2r::Publisher<TFMessage>,
    odom_puber: &r2r::Publisher<Odometry>,
    msg: PoseStamped,
    secs: i32,
    nsecs: u32,
) -> Result<(), Box<dyn std::error::Error>> {
    // Create shared timestamp
    let timestamp = r2r::builtin_interfaces::msg::Time {
        sec: secs,
        nanosec: nsecs,
    };
    
    // Create and publish TF message
    let tf_transform = TransformStamped {
        header: r2r::std_msgs::msg::Header {
            stamp: timestamp.clone(),
            frame_id: ODOM_FRAME.to_string(),
        },
        child_frame_id: BASE_LINK_FRAME.to_string(),
        transform: r2r::geometry_msgs::msg::Transform {
            translation: r2r::geometry_msgs::msg::Vector3 {
                x: msg.pose.position.x,
                y: msg.pose.position.y,
                z: msg.pose.position.z,
            },
            rotation: msg.pose.orientation.clone(),
        },
    };
    
    let pose = msg.pose;
    
    let tf_msg = TFMessage {
        transforms: vec![tf_transform],
    };
    
    tf_puber.publish(&tf_msg)
        .map_err(|e| format!("Failed to publish TF: {}", e))?;
    
    // Create and publish odometry message
    let odom_msg = Odometry {
        header: r2r::std_msgs::msg::Header {
            stamp: timestamp,
            frame_id: ODOM_FRAME.to_string(),
        },
        child_frame_id: BASE_LINK_FRAME.to_string(),
        pose: r2r::geometry_msgs::msg::PoseWithCovariance {
            pose,
            covariance: vec![0.0; 36], // Default covariance
        },
        twist: r2r::geometry_msgs::msg::TwistWithCovariance::default(), // Zero velocity
    };
    
    odom_puber.publish(&odom_msg)
        .map_err(|e| format!("Failed to publish odometry: {}", e).into())
}



/// Initialize and run the odometry demo node
fn main() -> Result<(), Box<dyn std::error::Error>> {
    // Initialize ROS2 context and node
    let ctx = r2r::Context::create()
        .map_err(|e| format!("Failed to create ROS2 context: {}", e))?;
    let mut node = r2r::Node::create(ctx, NODE_NAME, "")
        .map_err(|e| format!("Failed to create node '{}': {}", NODE_NAME, e))?;
    
    // Create publishers with error handling
    let odom_puber = node.create_publisher::<Odometry>(ODOM_TOPIC, QosProfile::default())
        .map_err(|e| format!("Failed to create odometry publisher on '{}': {}", ODOM_TOPIC, e))?;
    let tf_puber = node.create_publisher::<TFMessage>(TF_TOPIC, QosProfile::default())
        .map_err(|e| format!("Failed to create TF publisher on '{}': {}", TF_TOPIC, e))?;
    
    // Create subscribers with error handling
    let pose_sub = node.subscribe::<PoseStamped>(POSE_TOPIC, QosProfile::default())
        .map_err(|e| format!("Failed to create pose subscriber on '{}': {}", POSE_TOPIC, e))?;
    
    // Print startup information
    println!("🤖 {} node started successfully", NODE_NAME);
    println!("📡 Subscribing to: {}", POSE_TOPIC);
    println!("📤 Publishing to: {} and {}", ODOM_TOPIC, TF_TOPIC);
    println!("🔄 Ready to process pose messages...");
    
    // Setup async task execution
    run_async_loop(node, pose_sub, odom_puber, tf_puber)
}

/// Run the main async event loop
fn run_async_loop(
    mut node: r2r::Node,
    pose_sub: impl futures::Stream<Item = PoseStamped> + Unpin + 'static,
    odom_puber: r2r::Publisher<Odometry>,
    tf_puber: r2r::Publisher<TFMessage>,
) -> Result<(), Box<dyn std::error::Error>> {
    // Create task pool for async operations
    let mut pool = LocalPool::new();
    let spawner = pool.spawner();
    
    // Clone publishers for async tasks
    let odom_puber_clone = odom_puber.clone();
    let tf_puber_clone = tf_puber.clone();
    
    // Spawn pose handling task
    spawner.spawn_local(async move {
        pose_sub.for_each(|msg| {
            if let Err(e) = handle_pose(msg, &odom_puber_clone, &tf_puber_clone) {
                eprintln!("❌ Error handling pose message: {}", e);
            }
            futures::future::ready(())
        }).await;
    }).map_err(|e| format!("Failed to spawn pose handling task: {}", e))?;
    
    // Main event loop with improved timing
    const SPIN_DURATION_MS: u64 = 10;
    let spin_duration = std::time::Duration::from_millis(SPIN_DURATION_MS);
    
    println!("🚀 Starting main event loop (spin interval: {}ms)", SPIN_DURATION_MS);
    
    loop {
        node.spin_once(spin_duration);
        pool.run_until_stalled();
    }
}